#include "bms.h"

unsigned int cellVoltage[12];


//packet error control
unsigned char PEC(unsigned char *buf, unsigned char length)
{
	unsigned char CRCByte = 0;
	unsigned int remainder;

	for (unsigned char i = 0; i < length; i++)
	{
		//Bitwise XOR the current CRC value with the buf byte
		remainder = CRCByte ^ buf[i];
		//Process each of the 8 Remainder bits
		for (unsigned char j = 8; j > 0; j--)
		{
			//Determine if MSb = 1 prior to left shift
			if ((remainder & 0x80) == 0x80)
				//When MSb = 1, left shift and XOR with 8 lsbs of the polynomial
				remainder = ((remainder << 1) ^ 0x7);
			else
				//When MSb = 0, left shift 1 bit
				remainder = remainder << 1;
			
			//Truncate the CRC value to 8 bits
			remainder = remainder & 0xFF;
			//Proceed to the next Remainder bit
		}
		CRCByte = remainder;
		//Operate on the next data byte in the buf
	}
	return CRCByte;
}

//powers up bms
void bmsEnable(unsigned char enable)
{
	if (enable)
		PORT_BMS_VCC|= BMS_VCC;
	else
		PORT_BMS_VCC&= ~BMS_VCC;
}

//starts a conversion of all cells, waitDone sets, if programs waits for conversion complete status
void bmsStartConversion(unsigned char waitDone)
{
		unsigned char commandBuf[4];	
		unsigned char i = 0;
		unsigned char timeout = 255;
		
		// ==========  CELLSCANEN ========== 
		commandBuf[0] = BROADCAST+I2C_WRITE;
		commandBuf[1] = CELLSCANEN;
		commandBuf[2] = 0xFF;	//low byte
		commandBuf[3] = 0x0F;	//high byte		//conversion of all 12 cells	
		if(i2c_start(commandBuf[0]) == 0) 
		{
			i2c_write(commandBuf[1]);
			i2c_write(commandBuf[2]);	
			i2c_write(commandBuf[3]);		
			i2c_write(PEC(commandBuf,4));
		}		
		i2c_stop();
		
		
		// ==========  ADCCFG ========== 
		commandBuf[0] = BROADCAST+I2C_WRITE;
		commandBuf[1] = ADCCFG;
		commandBuf[2] = OVSAMPL << 5;	//low byte
		commandBuf[3] = 0x00;			//high byte
		if(i2c_start(commandBuf[0]) == 0) 
		{
			i2c_write(commandBuf[1]);
			i2c_write(commandBuf[2]);		
			i2c_write(commandBuf[3]);			
			i2c_write(PEC(commandBuf,4));
		}		
		i2c_stop();		
		
		
		//SCANCTRL
		commandBuf[0] = BROADCAST+I2C_WRITE;
		commandBuf[1] = SCANCTRL;
		commandBuf[2] = 0x01;	//start scan, clear DATARDY
		commandBuf[3] = 0x00;		
		if(i2c_start(commandBuf[0]) == 0) 
		{
			i2c_write(commandBuf[1]);
			i2c_write(commandBuf[2]);			
			i2c_write(commandBuf[3]);
			i2c_write(PEC(commandBuf,4));
		}		
		i2c_stop();
		
		if (waitDone)
		{
			//wait for DATARDY bit		
			i = 0;
			while((!(i & (1<<5))) && timeout)
			{		
				//READ_ALL SCANCTRL
				if(i2c_start(BROADCAST+I2C_WRITE) == 0) 
				{
					if(i2c_write(SCANCTRL)== 0)
						if(i2c_rep_start(BROADCAST+I2C_READ) == 0)
						{
							i2c_readAck();		//low byte
							i = i2c_readNak();	//high byte
						}
				}			
				i2c_stop();
				delay_ms(1);
				timeout--;
			}
		}		
}

//reads individual cell voltage (-> needs a bmsStartConversion() first)
//return in mV (normed by SHIFT_BMS_VOLTAGE)
unsigned int bmsReadCell(unsigned char cell)
{
	unsigned char lowbyte = 0,highbyte = 0;
	unsigned int result = 0;
	long resLong = 0;
	int	resShort = 0;
	
	//READ_ALL, CELLx
	if(i2c_start(BROADCAST+I2C_WRITE) == 0) 
	{
		if(i2c_write(CELLREGBASE-1+cell)== 0)
			if(i2c_rep_start(BROADCAST+I2C_READ) == 0)
			{
				lowbyte = i2c_readAck();	
				highbyte = i2c_readNak();	
			}
	}
	i2c_stop();	
	
	
	result = ((((unsigned int) highbyte & 0xFF) << 4) | (((unsigned int) lowbyte & 0xFF) >> 4));
	
	resLong =   (long) result * BMS_VOLTAGE_GAIN;
    resShort =  (int)(resLong >> (15-SHIFT_BMS_VOLTAGE));
	// shiftr 15: Umwandlung 30bit Wert auf 15bit Wert
    // shfitl  SHIFT_BMS_VOLTAGE: Es wurde einem auf SHIFT_BMS_VOLTAGE normierten Wert multipliziert; 
	
	return(resShort);

}

// set individual balance switch 
// the switch is disabled automatically after timeout 
// timout is a 4-bit value, max = 60s
void bmsBalanceCell(unsigned int balCFG, unsigned char timeout)
{	
	unsigned char commandBuf[4];	
	unsigned int data;	
		
	// ==========  BALCFG ========== 
	//set 0xFFFF = all cells  balance enable
	commandBuf[0] = BROADCAST+I2C_WRITE;
	commandBuf[1] = BALCFG;
	commandBuf[2] = ((unsigned char) balCFG & 0xFF);		//low byte
	commandBuf[3] = ((balCFG >> 8) & 0x0F) | 0xF0;			//high byte
	if(i2c_start(commandBuf[0]) == 0) 
	{
		i2c_write(commandBuf[1]);
		i2c_write(commandBuf[2]);	
		i2c_write(commandBuf[3]);			
		i2c_write(PEC(commandBuf,4));
	}	
	i2c_stop();
	
	data = ((timeout & 0x0F) << 8)	| (1<<13);				//60s max	
	// ==========  ACQCFG ========== 
	commandBuf[0] = BROADCAST+I2C_WRITE;
	commandBuf[1] = ACQCFG;
	commandBuf[2] = ((unsigned char) data  & 0xFF);		//low byte
	commandBuf[3] = (data >> 8);							//high byte
	if(i2c_start(commandBuf[0]) == 0) 
	{
		i2c_write(commandBuf[1]);
		i2c_write(commandBuf[2]);	
		i2c_write(commandBuf[3]);			
		i2c_write(PEC(commandBuf,4));
	}	
	i2c_stop();
}

//enables the charge switch via GPIO
void bmsSetChargeEnable(unsigned char enable)
{
	unsigned char commandBuf[4];
	// ==========  GPIO ========== 
	commandBuf[0] = BROADCAST+I2C_WRITE;
	commandBuf[1] = GPIO;
	if (enable)
	{
		commandBuf[2] = (1<<0);						//low byte
		commandBuf[3] = (1<<4);						//high byte	
	}
	else
	{
		commandBuf[2] = 0;								//low byte
		commandBuf[3] = (1<<4);						//high byte	
	}
	
	if(i2c_start(commandBuf[0]) == 0) 
	{
		i2c_write(commandBuf[1]);
		i2c_write(commandBuf[2]);	
		i2c_write(commandBuf[3]);			
		i2c_write(PEC(commandBuf,4));
	}	
	i2c_stop();
} 

//Starts a complete conversion of all 12 cells and write the result to cellVoltage[i]
void bmsReadAll(void){
	bmsStartConversion(TRUE);
	
	for (unsigned char i=1;i<=12;i++){
		cellVoltage[i-1] = bmsReadCell(i);
	}
}

//must be called after a bmsCompleteConversion -
//enables balancing if cell voltage > (mean cell voltage + tresholdVoltage) or if maxVoltage is reached
//for timeout amount of time -> must be called peridically
//timout is a 4-bit value, max = 60s
//all voltage are normed by SHIFT_BMS_VOLTAGE
//returns balCFG register
unsigned int bmsBalanceAll(unsigned int tresholdVoltage, unsigned int maxVoltage, unsigned char timeout){
	unsigned long avCellVoltage=0;
	unsigned char i;
	unsigned char commandBuf[4];
	unsigned int balCFG=0;
	
	for (i=1;i<=12;i++)
		avCellVoltage+= cellVoltage[i-1];
	
	avCellVoltage = avCellVoltage / 12;
	
	for (i=1;i<=12;i++)
	{
		if ((cellVoltage[i-1] > ((unsigned int) avCellVoltage + tresholdVoltage)) || (cellVoltage[i-1] >= maxVoltage))
			balCFG|= (1<<(i-1));
		else
			balCFG&= ~(1<<(i-1));
	}
		
	// ==========  BALCFG ========== 
	//set 0xFFFF = all cells  balance enable
	commandBuf[0] = BROADCAST+I2C_WRITE;
	commandBuf[1] = BALCFG;
	commandBuf[2] = ((unsigned char) balCFG & 0xFF);		//low byte
	commandBuf[3] = ((balCFG >> 8) & 0x0F) | 0xF0;			//high byte
	if(i2c_start(commandBuf[0]) == 0) 
	{
		i2c_write(commandBuf[1]);
		i2c_write(commandBuf[2]);	
		i2c_write(commandBuf[3]);			
		i2c_write(PEC(commandBuf,4));
	}	
	i2c_stop();
	
	
	//write timeout
	unsigned int data;
	data = ((timeout & 0x0F) << 8)	| (1<<13);				//60s max
	
	// ==========  ACQCFG ========== 
	commandBuf[0] = BROADCAST+I2C_WRITE;
	commandBuf[1] = ACQCFG;
	commandBuf[2] = ((unsigned char) data & 0xFF);			//low byte
	commandBuf[3] = (data >> 8);			//high byte
	if(i2c_start(commandBuf[0]) == 0) 
	{
		i2c_write(commandBuf[1]);
		i2c_write(commandBuf[2]);	
		i2c_write(commandBuf[3]);			
		i2c_write(PEC(commandBuf,4));
	}	
	i2c_stop();
	
	return (balCFG);
}

//check for bms to be ready (powered up by charge connector)
unsigned char bmsReady(void){
	if(i2c_start(BROADCAST+I2C_READ) == 0) 
	{
		i2c_stop();
		return(TRUE);
	}
	else
	{
		i2c_stop();
		return(FALSE);
	}
	
	
}



//backup

/* / ========== BROADCAST ========== 
		if(i2c_start(BROADCAST+I2C_WRITE) == 0) 
			txBuffer[0] = 123;
		else
			txBuffer[0] = 0;
		i2c_stop();
		
		spiMasterReadWrite(txBuffer,rxBuffer,1);		
		*/
		
		
		/*
		// ========== ROLL_ALL ========== 
		if(i2c_start(BROADCAST+I2C_WRITE) == 0) 
			txBuffer[0] = 123;
			
			if(i2c_write(ROLLALL+I2C_READ)== 0)
				if(i2c_rep_start(BROADCAST+I2C_READ) == 0)
				{
					txBuffer[2] = i2c_readAck();	//this is the first devices' address
					i2c_readAck();
					i2c_readAck();
					i2c_readNak();
				}	
		else
			txBuffer[0] = 0;
		
		i2c_stop();
		
		spiMasterReadWrite(txBuffer,rxBuffer,4);		
		delay_ms(2000);
		
		*/